{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "%pylab inline\n",
    "from sympy import init_printing,Matrix\n",
    "init_printing(use_latex='mathjax')\n",
    "connect_robot = False\n",
    "from robot import *\n",
    "import almath\n",
    "from nao import Nao"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nao = Nao()\n",
    "if connect_robot:\n",
    "    nao.connect_red()\n",
    "    nao.simulationInitPose()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from nao_kinematics import fLeftHand,fLeftLeg,\\\n",
    "    fRightHand,fRightLeg,fDownCamera,fUpCamera,\\\n",
    "    iLeftHand, iRightHand"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 验证正拟运动学"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get2(func,val):\n",
    "    if connect_robot:\n",
    "        return func()\n",
    "    else:\n",
    "        return val"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### LArm "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "angles = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142,0.0]\n",
    "angles = get2(nao.getLArmAngles,angles)\n",
    "angles = angles[:5]\n",
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf = almath.Transform([0.967762, 0.00177877, 0.251861, 0.18056,\n",
    "       -0.251867, 0.00542309, 0.967747, 0.0773332,\n",
    "       0.000355533, -0.999984, 0.00569627, 0.026255])\n",
    "Tf = get2(nao.getLArmTf,Tf)\n",
    "Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Pos = nao.tf2Pos(Tf)\n",
    "Pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "正运动学验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf, Pos = fLeftHand(*angles)\n",
    "nao.nparray2Tf(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nao.nparray2Pos(Pos*0.001) # 单位变换"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "逆运动学验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "iLeftHand(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(fLeftHand(*iLeftHand(Tf))[0] - Tf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### RArm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "angles = [0.78600859642,-0.0466810427606,1.17838907242,0.835045814514,0.299782037735,0.0]\n",
    "angles = get2(nao.getRArmAngles,angles)\n",
    "angles = angles[:5]\n",
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf = almath.Transform([0.967762, -0.0017788, 0.251861, 0.18056,\n",
    "           0.251867, 0.00542308, -0.967747, -0.0773332,\n",
    "           0.00035557, 0.999984, 0.00569628, 0.026255])\n",
    "Tf = get2(nao.getRArmTf,Tf)\n",
    "Tf"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Pos = nao.tf2Pos(Tf)\n",
    "Pos"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "正运动学验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf, Pos = fRightHand(*angles)\n",
    "nao.nparray2Tf(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "nao.nparray2Pos(Pos*0.001) # 单位变换"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "逆运动学验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "iRightHand(Tf)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Matrix(fRightHand(*iRightHand(Tf))[0] - Tf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 实验"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "angles = [0.786008536816,0.0466809943318,-1.17838895321,-0.835045695305,-0.299782127142,0.0]\n",
    "angles = get2(nao.getLArmAngles,angles)\n",
    "angles = angles[:5]\n",
    "angles"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf,Pos = fLeftHand(*angles[0:5])\n",
    "Matrix(fLeftHand(*iLeftHand(Tf))[0] - Tf) # 正逆运动学验证"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "Tf[0,3] += 50 # x 方向增加 50mm\n",
    "Matrix(fLeftHand(*iLeftHand(Tf))[0] - Tf) # 正逆运动学验证"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "也就是说,如果指定的位姿达不到,那么即使,逆运动学正确,解出来的关节角也不能达到.\n",
    "\n",
    "这是因为 Nao 机械臂是5个自由度,不能解出六个自由度的空间任意姿态.\n",
    "\n",
    "那么,如果是要求末端姿态为在指定位置,绕指定轴转动,不限制转动的角度,则问题变为5个自由度的空间解,\n",
    "\n",
    "通过最优化方法求得转动的theta角"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
